#pragma ident "$Id: SolverGFIF.cpp 2877 2011-08-20 08:40:11Z yanweignss $"

/**
 * @file SolverGFIF.hpp
 * General Solver.
 */

//============================================================================
//
//  Revision
//
//  2019/08/07
//  create a general solver for multi-gpss ppp solution
//
//============================================================================


#include "SolverGFIF.hpp"
#include "SystemTime.hpp"

#define debug 0

namespace gpstk
{

      // Index initially assigned to this class
   int SolverGFIF::classIndex = 9600000;

      // Returns an index identifying this object.
   int SolverGFIF::getIndex() const
   { return index; }

      // Returns a string identifying this object.
   std::string SolverGFIF::getClassName() const
   { return "SolverGFIF"; }

   void SolverGFIF::Init(void)
   {
       // set up variables and equations for PPP

       //////////////////////////////////////////////////////////////
       //
       // GFIF(1,2,5) = IF(1,2) - IF(1,5) = ifcb + const;
       //
       //////////////////////////////////////////////////////////////

   }

   void SolverGFIF::setUpEquations(void)
   {

       ///////////////////////////////////////////////
       // Now, define variables need to be estimated 
       ///////////////////////////////////////////////
       
       // Ifcb
       Variable ifcb( TypeID::ifcbSat, &whiteNoiseModel, false, true, true, 1.0);
       // amb
       Variable amb( TypeID::BLC, &ambiModel, true, true, true, 1.0 );

       //
       // Define Equation object for code/phase observations, and add variables 
       Equation equIFCB( TypeID::GFIF);

       //////////////////////////
       // insert variables now
       //////////////////////////
       equIFCB.addVariable(ifcb); 
       equIFCB.addVariable(amb);  

          ///////////
          // SETTING THE RULES: SETUP EQUATION SYSTEM
          ///////////
       
       // YOU MUST clear the equation system firstly!!!!!
       // only current equations is feed to equSystem
       equSystem.clearEquations();
       equSystem.addEquation(equIFCB);

   } // end of setUpEquations




      /* Code to be executed before 'Compute()' method.
       *
       * @param gData    Data object holding the data.
       */
   sourceTypeValueMap& SolverGFIF::preCompute( CommonTime& epoch,
                                                const SatID& sat,
                                      sourceTypeValueMap& srcTypeValueData)
       noexcept(false)
   {
      try
      {
         if(debug)
         {
             cout << "Begin to process sat:" << sat << endl;
         }

         YDSTime time(epoch);

         currSat = sat;

         // firstly, you must convert sourceTypeValueMap to soucreRinex for equation System
         sourceDataMap sData;
         for(sourceTypeValueMap::iterator srcIt=srcTypeValueData.begin();
                 srcIt!=srcTypeValueData.end();
                 srcIt++)
         {
             SourceID source = (*srcIt).first;
             typeValueMap tvData = (*srcIt).second;
             satTypeValueMap stvData;
             stvData[sat] = tvData;
             sData[source] = stvData;
         }

         sourceRinex sRin;
         sRin.body = sData;

         SourceIDSet sourceSet=sRin.body.getSourceIDSet();
         SatIDSet satSet = sRin.body.getSatIDSet();

            // Prepare the equation system with current data
         equSystem.Prepare(sRin);

            // Get the number of unknowns being processed
         int numEqu( equSystem.getEquNumber() );
         int numUnknowns( equSystem.getTotalNumVariables() );

            // Get the set with unknowns being processed
         VariableSet unkSet( equSystem.getVarUnknowns() );

         if(debug)
         {
             cout << "unkSet are as follows:" << endl;
             for(VariableSet::iterator vIt=unkSet.begin();
                     vIt!=unkSet.end();
                     vIt++)
             {
                 cout << (*vIt) << endl;
             }
         }


         varIndexMap.clear();

            // Feed the filter with the correct state and covariance matrix
         if(firstTime)
         {

            Vector<double> initialState(numUnknowns, 0.0);
            Matrix<double> initialCov( numUnknowns, numUnknowns, 0.0 );

               // Fill the initialCov matrix
            int i(0);      // Set an index
            for( VariableSet::const_iterator itVar = unkSet.begin();
                 itVar != unkSet.end();
                 ++itVar )
            {
               initialCov(i,i) = (*itVar).getInitialVariance();
               varIndexMap[(*itVar)] = i;
               ++i;
            }
               // Reset Kalman filter state and covariance matrix
            xhat = initialState; 
            P    = initialCov;

               // No longer first time
            firstTime = false;

         }
         else
         {

               // Adapt the size to the current number of unknowns
            Vector<double> currentState(numUnknowns, 0.0);
            Matrix<double> currentCov(numUnknowns, numUnknowns, 0.0);


               // Fill the state vector
            int i(0);      // Set an index
            for( VariableSet::const_iterator itVar = unkSet.begin();
                 itVar != unkSet.end();
                 ++itVar )
            {
               currentState(i) = stateMap[ (*itVar) ];
               varIndexMap[(*itVar)] = i;
               ++i;
            }

               // Fill the covariance matrix, 2019/08/01, shjzhang

               // We need a copy of 'unkSet'
            VariableSet tempSet( unkSet );

            i = 0;         // Reset 'i' index
            for( VariableSet::const_iterator itVar1 = unkSet.begin();
                 itVar1 != unkSet.end();
                 ++itVar1 )
            {
                int j(i);
                for( VariableSet::const_iterator itVar2 = tempSet.begin();
                     itVar2 != tempSet.end();
                     ++itVar2 )
                {
                    if( oldUnknowns.find(*itVar1) != oldUnknowns.end())
                    {
                        if( oldUnknowns.find(*itVar2) != oldUnknowns.end() )
                        {
                           currentCov(i, j) =
                              currentCov(j, i) =
                                 covMap[ (*itVar1) ][ (*itVar2) ];
                        }
                    }
                    else
                    {
                        // only diagnol element is assigned to initial variance
                        currentCov(i, i) = (*itVar1).getInitialVariance();
                    }

                    ++j;
                }

                ++i;

                // at last erase (*itVar1) for next loop
                tempSet.erase( (*itVar1) );

            }  // End of for( VariableSet::const_iterator itVar1 = unkSet...'

               // Reset Kalman filter to current state and covariance matrix
            xhat = currentState;
            P    = currentCov;


         }  // End of 'if(firstTime)'

            // State Transition Matrix (PhiMatrix)
         phiMatrix = equSystem.getPhiMatrix();
         qMatrix = equSystem.getQMatrix();

         if(debug)
         {
         cout << "phiMatrix" << endl;
         cout <<  phiMatrix  << endl;

         cout << "qMatrix" << endl;
         cout <<  qMatrix  << endl;
         }


         // save old unknowns for next epoch
         oldUnknowns = unkSet;


      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + StringUtils::asString( getIndex() ) + ":"
                                + u.what() );

         GPSTK_THROW(e);

      }

      return srcTypeValueData;

   }  // End of method 'SolverGFIF::preCompute()'



      // Compute the solution of the given equations set.
      //
      // @param gData    Data object holding the data.
      //
      // \warning A typical kalman filter will be devided into 
      // two different phase, the prediction and the correction.
      // for some special application, for example, the 'integer'
      // satellite and receiver clock/bias estimation, the ambiguity
      // constraints are generated using the 'predicted' ambiguity 
      // values and its variances. 
      //
   sourceTypeValueMap& SolverGFIF::Compute( CommonTime& epoch, 
                                            const SatID& sat,
                                            sourceTypeValueMap& srcTypeValueData )
       noexcept(false)
   {
      TimeUpdate( phiMatrix, qMatrix );
      Constraint();
      MeasUpdate( measVector, hMatrix, rMatrix );

         // Return  
      return srcTypeValueData;

   }  // End of method 'SolverGFIF::Compute()'


      // Predict the state vector and covariance matrix
      //
      // @param gData    Data object holding the data.
      //
   int SolverGFIF::TimeUpdate( const Matrix<double>& phiMatrix,
                                 const Matrix<double>& processNoiseCovariance )
       noexcept(false)
   {

         // Check whether the phiMatrix is square
      if (!(phiMatrix.isSquare()))
      {
         InvalidSolver e("phiMatrix is not square");
         GPSTK_THROW(e);
      }

         // Get the number of unknowns being processed
      int numUnknowns( equSystem.getTotalNumVariables() );
      int stateSize(xhat.size());
      if(!(stateSize==numUnknowns))
      {
         InvalidSolver e("TimeUpdate(): Size of a posteriori state estimation vector \
do not match the number of unknowns");
         GPSTK_THROW(e);
      }

      int phiRow = static_cast<int>(phiMatrix.rows());
      if (!(phiRow==numUnknowns))
      {
         InvalidSolver e("Number of unknowns does not match dimension \
of phiMatrix");
         GPSTK_THROW(e);
      }

      if (!(qMatrix.isSquare()))
      {
         InvalidSolver e("qMatrix is not square");
         GPSTK_THROW(e);
      }

      int qRow = static_cast<int>(qMatrix.rows());
      if (!(qRow==numUnknowns))
      {
         InvalidSolver e("Number of unknowns does not match dimension \
of qMatrix");
         GPSTK_THROW(e);
      }

      try
      {
            // Compute the a priori state vector
         xhatminus = phiMatrix*xhat;
         Matrix<double> phiT(transpose(phiMatrix));
            // Compute the a priori estimate error covariance matrix
         Pminus = phiMatrix*P*phiT + processNoiseCovariance;

      }
      catch(...)
      {
         InvalidSolver e("TimeUpdate(): Unable to predict next state.");
         GPSTK_THROW(e);
         return -1;
      }
        
      return 0;

   }  // End of method 'SolverGFIF::Compute()'


   int SolverGFIF::Constraint()
       noexcept(false)
   {

            // Get the number of unknowns being processed
         int numEqu( equSystem.getEquNumber() );
         int numUnknowns( equSystem.getTotalNumVariables() );

            // Get matrices and vectors out of equation system
            // Measurements vector (Prefit-residuals)
         measVector = equSystem.getPrefitsVector();
         hMatrix = equSystem.getGeometryMatrix();
         rMatrix = equSystem.getWeightsMatrix();

         int numAll(numEqu+1);
         Vector<double> tempPrefit(numAll, 0.0);
         Matrix<double> tempGeometry(numAll, numUnknowns, 0.0);
         Matrix<double> tempWeight(numAll, numAll, 0.0);

         for(int i=0; i<numEqu; i++)
         {
               // Prefit
            tempPrefit(i) = measVector(i);
               // Geometry
            for(int j=0; j<numUnknowns; j++)
            {
               tempGeometry(i,j) = hMatrix(i,j);
            }
               // Weight
            tempWeight(i,i) = rMatrix(i,i);
         }

            // Get the set with unknowns being processed
         VariableSet unkSet( equSystem.getVarUnknowns() );

            // Constraint of ambiguity datum 
         VariableSet ambSet;
         for(VariableSet::iterator varIt=unkSet.begin();
                 varIt!=unkSet.end();
                 varIt++)
         {
             if((*varIt).getType() == TypeID::BLC)
             {
                 ambSet.insert( (*varIt ) );
             }
         }

         // now,get the ambiguity with minimum variance
         double maxVar(1.0E+15);
         int maxIdx(0);
         Variable ambDatum;
         for( VariableSet::iterator ambIt=ambSet.begin();
                 ambIt != ambSet.end();
                 ambIt++ )
         {
             int ambIdx = varIndexMap[(*ambIt)];
             double ambVar = Pminus(ambIdx, ambIdx);
             if(ambVar<maxVar)
             {
                 maxIdx = ambIdx;
                 maxVar = ambVar;
                 ambDatum = (*ambIt);
             }
         }

         if(debug)
         {
            cout << "xhatminus:" << endl;
            cout << xhatminus << endl;

            cout << "Pminus:" << endl;
            cout << Pminus << endl;

            cout << "measVector:" << endl;
            cout << measVector << endl;

            cout << "hMatrix:" << endl;
            cout << hMatrix << endl;

            cout << "rMatrix:" << endl;
            cout << rMatrix << endl;

            cout << "datum amb:" << ambDatum << endl;
         }

         // now, the constraint
         tempPrefit(numEqu) = xhat(maxIdx);
         tempGeometry(numEqu, maxIdx) = 1.0;
         tempWeight(numEqu, numEqu) = 1.0E+10 ;

         // update hMatrix/rMatrix/measVector
         measVector = tempPrefit;
         hMatrix = tempGeometry;
         rMatrix = tempWeight;


      return 0;

   }  // End of method 'SolverGFIF::Constraint()'

      // Correct the state vector and covariance matrix
      //
      // @param gData    Data object holding the data.
      //
   int SolverGFIF::MeasUpdate( Vector<double>& prefitResiduals,
                                 Matrix<double>& designMatrix,
                                 Matrix<double>& weightMatrix )
       noexcept(false)
   {

         // By default, results are invalid
      valid = false;
      if (!(weightMatrix.isSquare()))
      {
         InvalidSolver e("MeasUpdate(): Weight matrix is not square");
         GPSTK_THROW(e);
      }


      int wRow = static_cast<int>(weightMatrix.rows());
      int pRow = static_cast<int>(prefitResiduals.size());
      if (!(wRow==pRow))
      {
         InvalidSolver e("MeasUpdate(): prefitResiduals size does not match dimension of \
weightMatrix");
         GPSTK_THROW(e);
      }


      int gRow = static_cast<int>(designMatrix.rows());
      if (!(gRow==pRow))
      {
         InvalidSolver e("MeasUpdate(): prefitResiduals size does not match \
dimension of designMatrix");
         GPSTK_THROW(e);
      }


      int gCol = static_cast<int>(designMatrix.cols());
      int aprioriStateRow = static_cast<int>(xhatminus.size());
      if (!(gCol==aprioriStateRow))
      {
         InvalidSolver e("MeasUpdate(): designMatrix size and a priori state \
estimation vector do not match.");
         GPSTK_THROW(e);
      }


      int pMCol = static_cast<int>(Pminus.cols());
      if ( pMCol != aprioriStateRow )
      {
         InvalidSolver e("MeasUpdate(): Sizes of a priori error covariance \
matrix and a priori state estimation vector do not match.");
         GPSTK_THROW(e);
      }


         // After checking sizes, let's do the real correction work
      Matrix<double> invPMinus;
      Matrix<double> designMatrixT( transpose(designMatrix) );

      try
      {
         invPMinus = inverseChol(Pminus);
      }
      catch(...)
      {
         InvalidSolver e("MeasUpdate(): Unable to compute invPMinus matrix.");
         GPSTK_THROW(e);
         return -1;
      }

      try
      {
         Matrix<double> invTemp( designMatrixT * weightMatrix * designMatrix +
                                 invPMinus );
            // Compute the a posteriori error covariance matrix
         P = inverseChol( invTemp );

            // Compute the a posteriori state estimation
         xhat = P * ( (designMatrixT * weightMatrix * prefitResiduals) + 
                      (invPMinus * xhatminus) );
      }
      catch(Exception e)
      {
         InvalidSolver eis("MeasUpdate(): Unable to compute xhat.");
         GPSTK_THROW(eis);
         return -1;
      }

      xhatminus = xhat;
      Pminus = P;

      solution = xhat;
      covMatrix = P;
         // Compute the postfit residuals Vector
      postfitResiduals = prefitResiduals - (designMatrix * solution);

      if(debug)
      cout << "postfitResiduals:1" << postfitResiduals << endl;

      Vector<double> postfitX( xhat - xhatminus );

      double VTPV(0.0),VxPVx(0.0);
      for( int i = 0; i < wRow; ++i)
      {
          VTPV += postfitResiduals(i)*postfitResiduals(i)*weightMatrix(i,i);
      }

      for( int i = 0; i< aprioriStateRow; ++i)
      {
          double VxP(0.0);

          for( int j = 0; j < aprioriStateRow; ++j)
          {
              VxP += postfitX(j)*invPMinus(j,i);
          }

          VxPVx += VxP*postfitX(i);
      }

      // Different from LS
      double sigma = std::sqrt( (VTPV + VxPVx)/wRow );
      for(int i=0; i < pRow;i++)
      {
          if(std::abs(postfitResiduals(i)) > 3*sigma)
          {
             weightMatrix(i,i) = 1.0E-10;
          }
      }

      try
      {
         Matrix<double> invTemp( designMatrixT * weightMatrix * designMatrix +
                                 invPMinus );

            // Compute the a posteriori error covariance matrix
         P = inverseChol( invTemp );

            // Compute the a posteriori state estimation
         xhat = P * ( (designMatrixT * weightMatrix * prefitResiduals) + 
                      (invPMinus * xhatminus) );
      }
      catch(Exception e)
      {
         InvalidSolver eis("MeasUpdate(): Unable to compute xhat.");
         GPSTK_THROW(eis);
         return -1;
      }

      xhatminus = xhat;
      Pminus = P;

      solution = xhat;
      covMatrix = P;

         // Compute the postfit residuals Vector
      postfitResiduals = prefitResiduals - (designMatrix * solution);
            
         // If everything is fine so far, then the results should be valid
      valid = true;

      return 0;

   }  // End of method 'SolverGFIF::MeasUpdate()'



      /* Code to be executed after 'Compute()' method.
       *
       * @param gData    Data object holding the data.
       */
   sourceTypeValueMap& SolverGFIF::postCompute( CommonTime& epoch,
                                                const  SatID& sat,
                                                sourceTypeValueMap& srcTypeValueData )
       noexcept(false)
   {
      try
      {
            // Clean up values in 'stateMap' and 'covMap'
         stateMap.clear();
         covMap.clear();

            // Get the set with unknowns being processed
         VariableSet unkSet( equSystem.getVarUnknowns() );
            // Store values of current state
         int i(0);      // Set an index
         for( VariableSet::const_iterator itVar = unkSet.begin();
              itVar != unkSet.end();
              ++itVar )
         {
            stateMap[ (*itVar) ] = solution(i);
            ++i;
         }


            // Store values of covariance matrix
            // We need a copy of 'unkSet'
         VariableSet tempSet( unkSet );
         i = 0;         // Reset 'i' index
         for( VariableSet::const_iterator itVar1 = unkSet.begin();
              itVar1 != unkSet.end();
              ++itVar1 )
         {
            int j(i);      // Set 'j' index
            for( VariableSet::const_iterator itVar2 = tempSet.begin();
                 itVar2 != tempSet.end();
                 ++itVar2 )
            {
               covMap[ (*itVar1) ][ (*itVar2) ] = covMatrix(i, j);
               ++j;
            }
            ++i;
            tempSet.erase( (*itVar1) ); // Remove current Variable from 'tempSet'
         }  // End of for( VariableSet::const_iterator itVar1 = unkSet...'

          // get dx/dy/dz values from state
         double ifcbSat(0.0);
         for(VariableDataMap::const_iterator vdIt = stateMap.begin();
             vdIt != stateMap.end();
             ++vdIt)
         {
             TypeID type=(*vdIt).first.getType();
             if(type==TypeID::ifcbSat)
             {
                 ifcbSat = (*vdIt).second;
                 if(firstSolution)
                 {
                     firstIfcb = ifcbSat;
                     firstSolution= false;
                 }
                 ifcbSat = ifcbSat - firstIfcb;
             }
         }

         // current 
//       const double bg15( 1.0/(GAMMA_GPS_L1L5 - 1.0) );
//       currentIfcb = ifcbSat/(-1.0*bg15);
         currentIfcb = ifcbSat;

      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + StringUtils::asString( getIndex() ) + ":"
                                + u.what() );

         GPSTK_THROW(e);

      }

      return srcTypeValueData;

   }  // End of method 'SolverGFIF::postCompute()'



   // print float solutions
   double SolverGFIF::getCurrentIFCB()
   {
       return currentIfcb;
   }

}  // End of namespace gpstk
